from motors.feetech import FeetechMotorsBus
from motors.configs import FeetechMotorsBusConfig
from motors.feetech import TorqueMode

# 配置串口参数和电机信息
config = FeetechMotorsBusConfig(
    port="/dev/ttyACM0",  # 串口端口
    motors={
            # name: (index, model)
            "shoulder_pan": [1, "sts3215"],
            "shoulder_lift": [2, "sts3215"],
            "elbow_flex": [3, "sts3215"],
            "wrist_flex": [4, "sts3215"],
            "wrist_roll": [5, "sts3215"],
            "gripper": [6, "sts3215"],
            }
)

# 初始化电机总线并建立串口连接
motors_bus = FeetechMotorsBus(config)
motors_bus.connect()  # 打开串口，建立通信

# 读取单个电机的当前位置（例如"shoulder_pan"）
# position = motors_bus.read("Present_Position", "shoulder_pan")
# print(f"当前位置: {position}")

# # 批量读取多个电机的状态
# while 1:
#     all_positions = motors_bus.read("Present_Position")
#     print(all_positions)


# 使能电机扭矩（必须先使能才能控制运动）
motors_bus.write("Torque_Enable", TorqueMode.ENABLED.value, "shoulder_pan")

# 发送目标位置指令（控制机械臂关节转动）
target_position = 3048  # 目标位置（根据校准后的范围设置）
motors_bus.write("Goal_Position", target_position, "gripper")
